import oscP5.*;
import netP5.*;

class KinectControlMove{
  
  KinectVectorTracker kvector1;
  KinectVectorTracker kvector2;
  OscP5 oscP5;
  NetAddress myRemoteLocation;
  
  PVector vectorRight;
  PVector vectorLeft;
  boolean oneHand = true;
  boolean twoHand = false;
  int i;
  int located;
  int ancientValue;
  
  KinectControlMove(Object object){
    vectorRight = new PVector(0,0);
    vectorLeft = new PVector(0,0);  
    kvector1 = new KinectVectorTracker();
    kvector2 = new KinectVectorTracker();
    
    oscP5 = new OscP5(object,10000);
    myRemoteLocation = new NetAddress("127.0.0.1",10000);
    
    i = 0;
    located = 0;
    ancientValue = 0;
  }
  
  String start(PVector locationRight,PVector locationLeft,boolean noTracked)
  { 
    String string = "None";
    if(!noTracked)
    {
      if(i%10==1){
        kvector1.setNewPosition(locationRight);
        vectorRight = kvector1.getVec();
        
        kvector2.setNewPosition(locationLeft);
        vectorLeft = kvector2.getVec();
        string = controlMove();
        int x = (int)Math.abs(locationRight.x-locationLeft.x);
       // string +=x;
        if(x>100)
        {
           // mode deux mains
          twoHand = true; 
        }
        if(!string.equals("None"))
        {
          if(twoHand == true && x<100){
            string = "transition";
            twoHand = false;
          }
          sendMessage(string);
        }
      }  
      i++;   
    }
    return string;
  }
  
  String controlMove(){
    String message = "None";
    
    if(moveSix())
      message = "/static_hand";
    if(moveThree())
      message = "/hand_up";
    else if(moveFive())
      message ="/hand_approach";
    else if(moveFour())
      message ="/hand_away";
    else if(moveSeven())
      message ="/hand_up";
    else if(moveOne())
      message = "/previous";
    else if(moveTwo())
      message = "/next";
      
    return message;
  }
  
  void sendMessage(String string){
    OscMessage myMessage = new OscMessage(string);
    oscP5.send(myMessage, myRemoteLocation);
  }
  
  void init(){
    //located = 0;
  }
  
  boolean moveSix(){
       if(vectorRight.x<ancientValue+50)
        {
          if(vectorRight.x>ancientValue-50)
            located++;
          else
            located=0;
        }
        else
          located = 0;
        ancientValue = (int)vectorRight.x;
        if(located==5)
        {
          located = 0;
          return true;
        }
        else
          return false;      
  }
  
  boolean moveOne(){
    return compareTo(vectorRight,new PVector(1,0));
  }
  
  boolean moveTwo(){
    init();
    return compareTo(vectorLeft,new PVector(-1,0));
  }
  
   boolean moveSeven(){
    if(twoHand){
    if(compareTo(vectorRight,new PVector(0,-1))&&compareTo(vectorLeft,new PVector(0,-1)))
      return true;
    else
      return false;
    }
    else
      return false;
  }
  
  boolean moveThree(){
    float angleRad = PVector.angleBetween(vectorLeft,inverse(vectorRight));
    PVector inverse = inverse(vectorRight);
    if(vectorRight.x>0)
      if(vectorLeft.mag()>100 && vectorRight.mag()>100)
        if(convertRadianToDegre(angleRad)<20)
        { 
          init();
          return true;
        }
    return false;
  }
  
  boolean moveFour(){
    float angleRad = PVector.angleBetween(vectorLeft,inverse(vectorRight));
    PVector inverse = inverse(vectorRight);
    if(vectorRight.x<0)
      if(vectorLeft.mag()>20 && vectorRight.mag()>20)
        if(convertRadianToDegre(angleRad)<20)
        { 
          init();
          return true;
        }
    return false;
  }
  
  boolean moveFive(){
    float angleRad = PVector.angleBetween(vectorLeft,inverse(vectorRight));
    PVector inverse = inverse(vectorRight);
    if(vectorRight.x>0)
      if(vectorLeft.mag()>20 && vectorLeft.mag()<100 && vectorRight.mag()>20 && vectorRight.mag()<100)
        if(convertRadianToDegre(angleRad)<20)
        { 
          init();
          return true;
        }
    return false;
  }
  
  boolean compareTo(PVector vec,PVector v1)
  {
    if(vec.mag()>100)
    {
       float angleRad = vec.angleBetween(v1,vec);
       float angleDegre = convertRadianToDegre(angleRad);
       if(angleDegre<20)
         return true;
        else
          return false;
    }
    else
        return false;
  }
  PVector inverse(PVector v1)
  {
    return new PVector(-1*v1.x,-1*v1.y);
  }
  float convertRadianToDegre(float angleRad)
  {
    return angleRad*360/(2*PI);
  }
}
